#include "cia402.h"

#include "rtbus/rtbus.h"
#include "log/zuclog.h"
#include "jkutil/bitdataop.h"

#define CIA_DEBUG(fmt, ...) zuclog_debug("CIA402_SERVO%d", fmt, cfg_.devid, ##__VA_ARGS__)
#define CIA_INFO(fmt, ...) zuclog_info("CIA402_SERVO%d", fmt, cfg_.devid, ##__VA_ARGS__)

using rtdev::Cia402;

static constexpr uint32_t servo_obj_id(uint8_t id, uint32_t objid) { return 0x01000000 * id + (objid & 0xffffff); }
static const uint32_t kControlWordId = 0x604000;
static const uint32_t kTargetPosId = 0x607a00;
static const uint32_t kTorqOffsetId = 0x60b200;
static const uint32_t kModeOpId = 0x606000;
static const uint32_t kTargetTorqId = 0x607100;

static const uint32_t kStatusWordId = 0x604100;
static const uint32_t kActPosId = 0x606400;
static const uint32_t kActTorqId = 0x607700;
static const uint32_t kModeOpDisplayId = 0x606100;
enum StateWord
{
    NO_READY_SWON = 0x00,
    SWON_DIS,
    READY_SWON,
    SWON,
    OP_EN,
    QUICK_STOP_ACTIVE,
    FAULT_REACTION_ACTIVE,
    FAULT
};

static StateWord get_stateword(uint16_t sword)
{
    if ((sword & 0x004f) == 0x00)
    {
        return NO_READY_SWON;
    }
    else if ((sword & 0x004f) == 0x40)
    {
        return SWON_DIS;
    }
    else if ((sword & 0x006f) == 0x21)
    {
        return READY_SWON;
    }
    else if ((sword & 0x006f) == 0x23)
    {
        return SWON;
    }
    else if ((sword & 0x006f) == 0x27)
    {
        return OP_EN;
    }
    else if ((sword & 0x006f) == 0x07)
    {
        return QUICK_STOP_ACTIVE;
    }
    else if ((sword & 0x004f) == 0x0f)
    {
        return FAULT_REACTION_ACTIVE;
    }
    else if ((sword & 0x004f) == 0x08)
    {
        return FAULT;
    }
    return NO_READY_SWON;
}

static const char* get_stateword_name(uint16_t sword)
{
    switch (get_stateword(sword))
    {
    case NO_READY_SWON:
        return "no_ready_swon";
    case SWON_DIS:
        return "swon_dis";
    case READY_SWON:
        return "ready_swon";
    case SWON:
        return "swon";
    case OP_EN:
        return "op_en";
    case QUICK_STOP_ACTIVE:
        return "quick_stop_active";
    case FAULT_REACTION_ACTIVE:
        return "fault_reaction_active";
    case FAULT:
        return "fault";
    default:
        return "unknown";
    }
}

Cia402::Cia402()
{
    last_alive_ = 0;
    cmd_collision_lv_ = 0;
    is_dyn_forward = 0;
}
int Cia402::boot()
{
    //仅针对ethercat总线
    // bus_ = rtbus::BusMgr::get()->bus("robbus0");
    if (!is_sim())
    {
        rtbus::RtCommuItf::SlaveInfo info;
        info.slaveid = cfg_.devid;
        info.vendor_id = 0x00661100;
        info.product_code = 0x10003202;  //FIXME:对于MAX和双臂，这个值需要区分
        info.revision_number = 0x00000001;
        info.cycletime_us = 8000;
        info.shifttime_us = info.cycletime_us * 0.25;
        info.enable_dc = true;
        bus_->config_slave(info);
        // 对于MAX特殊处理多出来的那个从站节点
        // if (info.slaveid % 2 == 0 && info.product_code == 0x10003202)
        // {
        //     info.enable_dc = false;
        //     info.slaveid = cfg_.devid + 1;
        //     bus_->config_slave(info);
        // }
    }
    bus_->config_txpdo(servo_obj_id(cfg_.devid, kControlWordId), 2);
    bus_->config_txpdo(servo_obj_id(cfg_.devid, kTargetPosId), 4);
    bus_->config_txpdo(servo_obj_id(cfg_.devid, kTorqOffsetId), 2);
    bus_->config_txpdo(servo_obj_id(cfg_.devid, kModeOpId), 1);
    bus_->config_txpdo(servo_obj_id(cfg_.devid, kTargetTorqId), 2);

    bus_->config_rxpdo(servo_obj_id(cfg_.devid, kStatusWordId), 2);
    bus_->config_rxpdo(servo_obj_id(cfg_.devid, kActPosId), 4);
    bus_->config_rxpdo(servo_obj_id(cfg_.devid, kActTorqId), 2);
    bus_->config_rxpdo(servo_obj_id(cfg_.devid, kModeOpDisplayId), 1);

    return 0;
}
uint16_t Cia402::handle_control_word(uint16_t sword, bool has_ext_enable_bit)
{
    uint16_t cword = 0;
    uint8_t* cword_ptr = (uint8_t*)&cword;

    auto cur_state = get_stateword(sword);
    bool iserror = false;
    bool stat_enable = false;
    switch (cur_state)
    {
    case NO_READY_SWON:
        cword = 0x00;
        break;
    case SWON_DIS:
        cword = 0x06;  //shutdown
        break;
    case READY_SWON:
        cword = this->cmd_enable_ ? 0x07 : 0x06;
        break;
    case SWON:
        cword = this->cmd_enable_ ? 0x0f : 0x06;
        break;
    case OP_EN: {
        stat_enable = true;
        cword = this->cmd_enable_ ? 0x0f : 0x06;
        //在支持扩展使能位的情况下，需要额外判定
        if (has_ext_enable_bit && (sword >> 14 & 0x01) == 0)
        {
            stat_enable = false;
        }

        break;
    }
    case QUICK_STOP_ACTIVE:
        cword = 0x06;  //shutdown
        break;
    case FAULT_REACTION_ACTIVE:
        iserror = true;
        break;

    case FAULT: {
        iserror = true;
        cword = this->cmd_.controlword;
        if (this->cmd_.error_reset)
        {
            Util::set_bit(cword_ptr, 7);
            this->cmd_.error_reset = false;
        }
        else
        {
            Util::clr_bit(cword_ptr, 7);
        }
        break;
    }
    default:
        break;
    }
    stat_.error_sta = iserror ? 1 : 0;

    //碰撞状态处理
    stat_.act_collision_lv = sword >> 15 & 0x01;
    if (stat_.act_collision_lv)
    {
        if (cmd_.collision_reset > 0)
        {
            Util::set_bit(cword_ptr, 11);  //cword |= (1 << 11);
            cmd_.collision_reset--;
        }
        else
        {
            Util::clr_bit(cword_ptr, 11);
        }
    }

    if (stat_enable != stat_.enable)
    {
        CIA_DEBUG("enable status change %d->%d", stat_.enable, stat_enable);
        stat_.enable = stat_enable;
    }

    //回零状态处理
    if (stat_.opmode_display == (int)ServoItf::TRACKING_MODE::OPMODE_HOME)
    {
        if (cmd_.home)
        {
            cword |= 0x10;
            cmd_.home = 0;
        }
        if ((sword & 0x3400) == 0x1400)  //home reached
        {
            Util::clr_bit(cword_ptr, 12);
            stat_.home = 1;
        }
    }
    return cword;
}
int Cia402::shutdown() { return -1; }
int Cia402::set_send_data(int busid)
{
    if (is_sim())
        return 1;
    if (!bus_.get())
        return -1;
    last_alive_ = is_alive();
    check_alive();
    auto tmp_cword = handle_control_word(stat_.statusword, 0);
    if (cmd_.controlword != tmp_cword)
    {
        CIA_DEBUG("cword 0x%x->0x%x", cmd_.controlword, tmp_cword);
    }
    cmd_.controlword = tmp_cword;

    bus_->set_txpdo_data(servo_obj_id(cfg_.devid, kControlWordId), (uint8_t*)&cmd_.controlword);

    uint8_t mode = 8;  //cmd_.opmode & 0x0f;
    // mode = cmd_.opmode & 0x0f;
    // mode |= (cmd_collision_lv_ & 0x07) << 4;
    // mode |= (is_dyn_forward & 0x01) << 7;
    bus_->set_txpdo_data(servo_obj_id(cfg_.devid, kModeOpId), (uint8_t*)&mode);

    if (cmd_.opmode == (int)ServoItf::TRACKING_MODE::OPMODE_CSP)
    {
        bus_->set_txpdo_data(servo_obj_id(cfg_.devid, kTargetPosId), (uint8_t*)&cmd_.target_pos);
    }
    else if (cmd_.opmode == (int)ServoItf::TRACKING_MODE::OPMODE_CST)
    {
        bus_->set_txpdo_data(servo_obj_id(cfg_.devid, kTargetTorqId), (uint8_t*)&cmd_.target_torq);
    }
    bus_->set_txpdo_data(servo_obj_id(cfg_.devid, kTorqOffsetId), (uint8_t*)&cmd_.torq_offset);

    return 0;
}
int Cia402::get_recv_data(int busid)
{
    if (is_sim())
        return 1;
    if (!bus_.get())
        return -1;
    //FIXME：后面和jkservo的处理合并
    int16_t status_word = 0;
    auto alive = bus_->get_rxpdo_data(servo_obj_id(cfg_.devid, kStatusWordId), (uint8_t*)&status_word);
    keep_alive(alive);
    if (status_word != stat_.statusword)
    {
        CIA_DEBUG("status %s(0x%x)->%s(0x%x)", get_stateword_name(stat_.statusword), stat_.statusword, get_stateword_name(status_word), status_word);
        stat_.statusword = status_word;
    }

    bus_->get_rxpdo_data(servo_obj_id(cfg_.devid, kActPosId), (uint8_t*)&stat_.act_pos);
    bus_->get_rxpdo_data(servo_obj_id(cfg_.devid, kActTorqId), (uint8_t*)&stat_.act_torq);
    stat_.opmode_display = 8;  //TODO:固件暂未支持反馈，所以暂时写死
    bus_->get_rxpdo_data(servo_obj_id(cfg_.devid, kModeOpDisplayId), (uint8_t*)&stat_.opmode_display);

    //TODO:OTA数据
    //TODO：错误码

    return 0;
}
int Cia402::get_sdo_blocking(uint32_t objid, void* data, int s, int retry)
{
    if (is_sim())
        return 0;
    return bus_->get_sdo_blocking(1 + cfg_.devid, objid, data, s, retry);
}
int Cia402::set_sdo_blocking(uint32_t objid, void* data, size_t s, void* readback, int retry)
{
    if (is_sim())
        return 0;
    return bus_->set_sdo_blocking(1 + cfg_.devid, objid, data, s, readback, retry);
}
int Cia402::set_opt(std::string opt_name, void* data, size_t s)
{
    if (opt_name == "torq-enable")
    {
        is_dyn_forward = *(uint8_t*)data;
    }
    else if (opt_name == "collision-level")
    {
        cmd_collision_lv_ = *(uint8_t*)data;
    }
    return 0;
}
bool Cia402::is_enable()
{
    if (is_sim())
        return cmd_enable_;
    return stat_.enable;
}
bool Cia402::is_homed() { return false; }
uint32_t Cia402::get_act_collision_lv() { return 0; }
bool Cia402::is_tor_overload() { return false; }
bool Cia402::is_backdrive() { return false; }
bool Cia402::is_collision() { return false; }